#include "opencv2/video/tracking.hpp"
#include "opencv2/highgui/highgui.hpp"

#include "../../Common/defines.h"

#include <stdio.h>
#include <iostream>
#include <iomanip>

#pragma comment(lib, "opencv_core242d")
#pragma comment(lib, "opencv_highgui242d")
#pragma comment(lib, "opencv_video242d")

using namespace cv;
using namespace std;

static inline void drawCross(Mat & img, Point center, Scalar color, int d)
{
	line(	img, 
		Point( center.x - d, center.y - d ),
		Point( center.x + d, center.y + d ), 
		color, 1, CV_AA, 0);

	line(	img, 
		Point( center.x + d, center.y - d ),
		Point( center.x - d, center.y + d ), 
		color, 1, CV_AA, 0 );
}

static inline void plotVector(Mat & img, vector<float> & v, float scaleX = 1.0, float scaleY = 1.0, Scalar c = COLOR_GREEN, int tp = 0)
{
	if(tp == 0)
	{
		for(unsigned int k = 0; k < (v.size() - 1); k++)
		{
			int x1 = (int)(k * scaleX);
			int y1 = (int)(v[k] * scaleY);

			int x2 = (int)((k+1) * scaleX);
			int y2 = (int)(v[k+1] * scaleY);

			line(img, Point(x1, y1), Point(x2, y2), c, 1);
		}
	}

	if(tp == 1)
	{
		for(unsigned int k = 0; k < (v.size() - 1); k++)
		{
			int x1 = (int)(k * scaleX);
			int y1 = (int)(v[k] * scaleY);
			drawCross(img, Point(x1, y1), c, 3);
		}
	}
}

static inline void plotMat(Mat & m, string name)
{
	cout << name << endl;

	for(int i = 0; i < m.rows; i++ )
	{
		for(int j = 0; j < m.cols; j++ )
		{
			cout << setiosflags(ios::fixed) << setprecision(3) << m.at<float>(i,j) << "\t";
		}

		cout << endl;
	}

	cout << endl;
	cout << endl;
}

float randomN(int stddev)
{
	Mat dest(1,1,CV_32F);
	randn(dest, 0, stddev);
	return dest.at<float>(0,0);
}

int main(int, char**)
{
	printf( "\nExamle of using OpenCV's Kalman filter.\n");
	Mat diagramm = Mat::zeros(800, 800, CV_8UC3);

	/* Initialization */
	srand((int)time(NULL));
	KalmanFilter KF(4, 4, 0);

	float stopTime    = 3.0f;
	float sampleTime  = 0.05f;
	float startHeight = 50.0f;
	float g = 9.81f;

	vector<float> realValues;
	vector<float> measuredValues;
	vector<float> filteredValues;

	float currenttime = 0;
	while(currenttime < stopTime)
	{
		float heighValue = startHeight - 0.5f * g * (currenttime * currenttime);
		float measuredHeigh = heighValue + ( randomN(1) );

		realValues.push_back(heighValue);
		measuredValues.push_back(measuredHeigh);

		currenttime += sampleTime;
	}

	// Transition Matrix A
	KF.transitionMatrix = *(Mat_<float>(4, 4) <<	1, sampleTime, 0,		0,
		0,		1,		0,		0,
		0,		0,		1,  sampleTime,
		0,		0,		0,		1);
	// Matrix u 
	Mat u = *(Mat_<float>(4, 1)	<<	0,
		0, 
		-0.5 * g * sampleTime * sampleTime, 
		-g * sampleTime);

	// Matrix B
	KF.controlMatrix = Mat(4,4,CV_32F);
	setIdentity(KF.controlMatrix);

	// Matrix H
	setIdentity(KF.measurementMatrix);
	KF.measurementMatrix.at<float>(1,1) = 0.0f;
	KF.measurementMatrix.at<float>(3,3) = 0.0f;

	// Matrix R
	setIdentity(KF.measurementNoiseCov, Scalar::all(10));
	KF.measurementNoiseCov.at<float>(1,1) = 0.0f;
	KF.measurementNoiseCov.at<float>(3,3) = 0.0f;

	// Matrix Q
	setIdentity(KF.processNoiseCov, Scalar::all(1e-2));

	// Matrix P -> posteriori error estimate covariance matrix
	// init value
	setIdentity(KF.errorCovPost, Scalar::all(0));

	// set startPosition
	KF.statePost = *(Mat_<float>(4, 1)  << 0, 0, startHeight, 0);

	unsigned int cnt = 0;

	while(cnt < measuredValues.size() )
	{
		// estimation (prediction)
		KF.predict(u);

		// generate measurement
		Mat measurement = *(Mat_<float>(4, 1) << cnt, 0, measuredValues[cnt], 0);

		// propagation (correction)
		KF.correct(measurement);

		filteredValues.push_back(KF.statePost.at<float>(2,0));

		cnt++;
	}

	float xScale = 12.0f;
	float yScale = 12.0f;

	// plot values
	plotVector(diagramm, realValues, xScale, yScale, COLOR_GREEN);
	plotVector(diagramm, measuredValues, xScale, yScale, COLOR_RED, 1);
	plotVector(diagramm, filteredValues, xScale, yScale, COLOR_TUERKIS);

	flip(diagramm ,diagramm, 0);

	/* Plot infos */
	line(diagramm, Point(5, 10), Point(15, 10), COLOR_GREEN);
	putText(diagramm, "Real position", Point(25, 15), FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255, 255, 255),1);

	drawCross(diagramm, Point(10, 30), COLOR_RED, 3);
	putText(diagramm, "Measured position", Point(25, 35), FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255, 255, 255),1);

	line(diagramm, Point(5, 50), Point(15, 50), COLOR_TUERKIS);
	putText(diagramm, "Filtered position", Point(25, 55), FONT_HERSHEY_PLAIN, 1.0, CV_RGB(255, 255, 255),1);

	imshow("diagram", diagramm);
	waitKey();

	return 0;
}